function [R, K, B, N, Q, Omega0, sigmaSq, tau, percentageError, normFitErrRMS] = allanvariance(sensOut, tau0, SF, isAcc, varargin)
%ALLANVARIANCE 计算Allan方差
%
% Input Arguments
% # sensOut: m*n矩阵，m为采样个数，n为器件个数，若sensOutType为1则sensOut/SF单位为为rad（对于陀螺）或m/s（对于加速度计），若sensOutType为2则单位为rad/s（对于陀螺）或m/s^2（对于加速度计），若sensOutType为3则单位为rad（对于陀螺）或m/s（对于加速度计）
% # tau0: 标量，采样间隔时间，单位s
% # SF: 长度为n的向量，各器件的标度因数，仅用于sensOutType为1时
% # isAcc: 长度为n的逻辑向量，true表示对应的器件为加速度计，否则为陀螺，用于绘制曲线时确定单位，默认全为false
% # varargin: 包含以下“参数-数值”对：
%     sinFreq: 长度为n的元胞向量，每个元素为长度为k的向量（k为正弦信号频点个数），各器件正弦信号频率，单位Hz，默认为空
%     sensOutType: 标量，1或其它值表示sensOut为各采样间隔内的角度或比速度增量，2表示sensOut为各采样时刻的角速度或比力，3表示sensOut为各采样时刻的角度或比速度，默认为1
%     calcDataNumPerDecade: 标量，每10倍时间内进行计算的Allan方差数据点个数，数据点将在log轴上均匀分布，该值越大精度越高，所需的计算时间越长，设为0指定按每簇采样数每增加1计算1次数据点（最高精度），默认为50
%     fitMethod: 标量，1为最小二乘法，其它为LSNE法，默认为LSNE法
%     doFitTwiceForLSNE: 逻辑标量，是否使用二次拟合，仅对LSNE法有效，默认为true
%     fitPercentErrorThreshold: 长度为n的向量，各元素应在1/sqrt(2*n)至1/sqrt(2)之间，各器件参与拟合的估计误差阈值，估计误差高于此值的Allan方差数据点将不参与拟合计算，默认全为0.13
%     resultIsInConvUnit: 标量，true表示结果值的单位为常用单位（重力加速度采用9.8m/s^2），否则为国际单位制单位，默认为true
%     doRepeatIdentTauPts: 逻辑标量，true表示重复相同相关时间值的点，默认为true
%     doPlot: 逻辑标量，true表示绘制Allan方差曲线，默认为true
%
% Output Arguments
% # R: 1*n向量，各器件的斜坡系数的绝对值，单位rad/s^2（对于陀螺）或m/s^3（对于加速度计）
% # K: 1*n向量，各器件的角速度随机游走系数，单位rad/s^(3/2)（对于陀螺）或m/s^(5/2)（对于加速度计）
% # B: 1*n向量，各器件的零偏不稳定性系数，单位rad/s（对于陀螺）或m/s^2（对于加速度计）
% # N: 1*n向量，各器件的角度随机游走系数，单位rad/s^(1/2)（对于陀螺）或m/s^(3/2)（对于加速度计）
% # Q: 1*n向量，各器件的量化噪声，单位rad（对于陀螺）或m/s（对于加速度计）
% # Omega0: 1*n元胞向量，每个元素为1*n向量，各器件的各频率正弦波的幅值，单位rad/s（对于陀螺）或m/s^2（对于加速度计）
% # sigmaSq: m'*n矩阵（m'为实际进行计算的Allan方差数据点个数，由于取点方法原因，部分数据点的时间可能重复，因此m'可能小于由calcDataNumPerDecade推算的数据点个数），σ(τ)^2，单位分别为(rad/s)^2（陀螺）或(m/(s^2))^2（加速度计）
% # tau: m'*1向量，τ，单位分别为s
% # percentageError: m'*1向量，估计误差，无单位
% # normFitErrRMS: 1*n向量，各器件所有数据点上归一化拟合误差的均方根值，无单位
%
% References
% # Gyro and Accelerometer Panel of the IEEE Aerospace and Electronic Systems Society. IEEE Std 952-1997. IEEE Standard Specification Format Guide and Test Procedure for Single-Axis Interferometric Fiber Optic Gyros[S]. New York: Institute of Electrical and Electronics Engineers, Inc., 1998: 附录C
% # Brian E, Grantham P E, Mark A. Bailey. A Least-Squares Normalized Error Regression Algorithm with Application to the Allan Variance Noise Analysis Method[J]. Navigation and Control Technology Division, 2006: 750~756
% # 《应用导航算法工程基础》“Allan方差计算要点”

tic;
p = inputParser;
p.FunctionName = 'allanvariance';
addRequired(p, 'sensOut', @(x)(ismatrix(x)&&isnumeric(x)));
addRequired(p, 'tau0', @(x)(isscalar(x)&&isnumeric(x)));
addRequired(p, 'SF', @(x)(isvector(x)&&isnumeric(x)&&(length(x)==size(sensOut, 2))));
addOptional(p, 'isAcc', false(size(SF)), @(x)(isvector(x)&&islogical(x)));
addParameter(p, 'sinFreq', {}, @(x)(isvector(x)&&iscell(x)));
addParameter(p, 'sensOutType', 1, @(x)(isscalar(x)&&isnumeric(x)));
addParameter(p, 'calcDataNumPerDecade', 50, @(x)(isscalar(x)&&isnumeric(x)));
addParameter(p, 'fitMethod', 2, @(x)(isscalar(x)&&isnumeric(x)));
addParameter(p, 'doFitTwiceForLSNE', true, @(x)(isscalar(x)&&islogical(x)));
addParameter(p, 'fitPercentErrorThreshold', 0.13*ones(size(SF)), @(x)(isvector(x)&&isnumeric(x)));
addParameter(p, 'resultIsInConvUnit', true, @(x)(isscalar(x)&&islogical(x)));
addParameter(p, 'doRepeatIdentTauPts', true, @(x)(isscalar(x)&&islogical(x)));
addParameter(p, 'doPlot', true, @(x)(isscalar(x)&&islogical(x)));
parse(p, sensOut, tau0, SF, isAcc, varargin{:});

%% 计算角度采样
[nSamp, nSens] = size(p.Results.sensOut);
if p.Results.sensOutType == 3
    angleSampNum = nSamp;
    theta = p.Results.sensOut;
else
    % 为减少计算量，角增量采样被转换为角度采样；角速度采样被转换为角增量采样，再转换为角度采样
    % 角度采样和角增量采样的序号关系
    % 角度采样θ     k= 1   2   3     m-1  m  m+1    N-1  N  N+1
    %                  |---|---|......|---|---|......|---|---|
    % 角增量采样Δθ k=   1   2         m-1  m         N-1  N
    % 可见由N个角增量采样可以构成N+1个角度采样
    angleSampNum = nSamp + 1;
    Dtheta = NaN(nSamp, nSens);
    if p.Results.sensOutType == 2
        Dtheta = p.Results.sensOut * p.Results.tau0;
    else
        for i=1:nSens
            Dtheta(:, i) = p.Results.sensOut(:, i) / p.Results.SF(i);
        end
    end
    theta = zeros(angleSampNum, nSens);
    theta(2:end, :) = cumsum(Dtheta, 1);
end

%% 计算各相关时间上的Allan方差
AllanVarFullLength = floor((angleSampNum-1)/2); % 确保AllanVarFullLength<((angleSampNum)/2)
if p.Results.calcDataNumPerDecade == 0
    sigmaSq = NaN(AllanVarFullLength, nSens);
    tau = NaN(AllanVarFullLength, 1);
    for m=1:AllanVarFullLength
        tau(m, 1) = m;
        OMEGABar = nonadjdiff_blksub(theta, m) / (m * p.Results.tau0);
        ensemble = nonadjdiff_blksub(OMEGABar, m) .^ 2;
        sigmaSq(m, :) = mean(ensemble, 1) / 2;
        disp([num2str(m) ' of ' num2str(AllanVarFullLength) ' correlation time points calculated.']);
    end
else
    calcDataNum = round(log10(AllanVarFullLength) * p.Results.calcDataNumPerDecade);
    if (calcDataNum > AllanVarFullLength)
        calcDataNum = AllanVarFullLength;
    end
    log10Step = log10(AllanVarFullLength) / (calcDataNum-1);
    sigmaSq = NaN(calcDataNum, nSens);
    tau = NaN(calcDataNum, 1);
    actualCalcDataNum = 0;
    mLast = 0;
    sigmaSqLast = NaN;
    for i=1:calcDataNum
        m = round(10^((i-1)*log10Step));
        if m ~= mLast % 在m较小时可能相邻两次循环计算的m相同，避免重复计算以减少计算时间
            actualCalcDataNum = actualCalcDataNum + 1;
            tau(actualCalcDataNum, 1) = m;
            OMEGABar = nonadjdiff_blksub(theta, m) / (m * p.Results.tau0);
            ensemble = nonadjdiff_blksub(OMEGABar, m) .^ 2;
            sigmaSq(actualCalcDataNum, :) = mean(ensemble, 1) / 2;
            mLast = m;
            sigmaSqLast = sigmaSq(actualCalcDataNum, :);
            disp([num2str(m) ' of ' num2str(AllanVarFullLength) ' correlation time points calculated.']);
        elseif p.Results.doRepeatIdentTauPts % 在m较小时可能相邻两次循环计算的m相同，重复计算以提高高频段的拟合精度
            actualCalcDataNum = actualCalcDataNum + 1;
            tau(actualCalcDataNum, 1) = mLast;
            sigmaSq(actualCalcDataNum, :) = sigmaSqLast;
            disp([num2str(m) ' of ' num2str(AllanVarFullLength) ' correlation time points calculated.']);
        end
    end
    tau = tau(1:actualCalcDataNum, :);
    sigmaSq = sigmaSq(1:actualCalcDataNum, :);
end
percentageError = 1 ./ sqrt(2*((angleSampNum)./tau-1));
tau = tau * p.Results.tau0;

%% 数据拟合
H5Full = [tau.^2 tau ones(size(tau)) 1./tau tau.^(-2)];
R = NaN(1, nSens);
K = NaN(1, nSens);
B = NaN(1, nSens);
N = NaN(1, nSens);
Q = NaN(1, nSens);
Omega0 = cell(1, nSens);
fittedSigmaSq = NaN(size(sigmaSq));
normFitErrRMS = NaN(1, nSens);
tauFitThreshold = NaN(1, nSens);
for i=1:nSens
    if isempty(p.Results.sinFreq)
        nSin = 0;
    else
        nSin = length(p.Results.sinFreq{i});
    end
    nCoef = 5 + nSin;
    HFull = [H5Full NaN(length(tau), nSin)];
    for j=1:nSin
        HFull(:, 5+j) = (sin(pi*p.Results.sinFreq{i}(j)*tau).^2./(pi*p.Results.sinFreq{i}(j)*tau)).^2;
    end
    C = NaN(nCoef, 1);
    newNegCoefIdx = [];
    negCoefIdx = [];
    nonNegCoefIdx = 1:nCoef;
    firstIter = true;
    fitDataNum = find(percentageError<=p.Results.fitPercentErrorThreshold(i), 1, 'last');
    fitDataNum = fitDataNum(1, 1);
    tauFitThreshold(1, i) = tau(fitDataNum, 1);
    while firstIter || ~isempty(newNegCoefIdx)
        C(newNegCoefIdx, 1) = 0;
        H = HFull(1:fitDataNum, nonNegCoefIdx);
        if ~isempty(nonNegCoefIdx)
            if p.Results.fitMethod == 1 % 最小二乘法
                C(nonNegCoefIdx, 1) = (H'*H) \ (H'*sigmaSq(1:fitDataNum, i));
            else % LSNE法
                F = (H ./ (repmat(sigmaSq(1:fitDataNum, i), 1, length(nonNegCoefIdx)).^2))'; % REF2式17
                C(nonNegCoefIdx, 1) = (F*H) \ (F*sigmaSq(1:fitDataNum, i)); % REF2式19
                if p.Results.doFitTwiceForLSNE
                    fittedSigmaSqLSNE = H * C(nonNegCoefIdx, 1);
                    F = (H ./ (repmat(fittedSigmaSqLSNE, 1, length(nonNegCoefIdx)).^2))'; % REF2式21
                    C(nonNegCoefIdx, 1) = (F*H) \ (F*sigmaSq(1:fitDataNum, i));
                end
            end
        end
        newNegCoefIdx = find(C(:, 1)<0)';
        negCoefIdx = sort([negCoefIdx newNegCoefIdx]);
        nonNegCoefIdx = setdiff(1:nCoef, negCoefIdx);
        firstIter = false;
    end
    R(1, i) = sqrt(C(1, 1)*2);
    K(1, i) = sqrt(C(2, 1)*3);
    B(1, i) = sqrt(C(3, 1)/(2/pi*log(2)));
    N(1, i) = sqrt(C(4, 1));
    Q(1, i) = sqrt(C(5, 1)/3);
    Omega0{1, i} = sqrt(C(6:end, 1))';
    fittedSigmaSq(:, i) = HFull * C(:, 1);
    normFitErrRMS(1, i) = rms_cg((fittedSigmaSq(1:fitDataNum, i)-sigmaSq(1:fitDataNum, i))./sigmaSq(1:fitDataNum, i), 1);
end

%% 结果单位转换
if p.Results.resultIsInConvUnit
    g = 9.8;
    rad2Deg = 180 / pi;
    for i=1:nSens
        if p.Results.isAcc(i)
            R(1, i) = R(1, i) / (g/1e6) * 3600; % 转换前单位m/s^3，转换后单位μg/h
            K(1, i) = K(1, i) / (g/1e6) * 60; % 转换前单位m/s^(5/2)，转换后单位μg/sqrt(h)
            B(1, i) = B(1, i) / (g/1e6); % 转换前单位m/s^2，转换后单位μg
            N(1, i) = N(1, i) / (g/1e6) / 60; % 转换前单位m/s^(3/2)，转换后单位μg*sqrt(h)
            Q(1, i) = Q(1, i) * 3600; % 转换前单位m/s，转换后单位m/h
            Omega0{1, i} = Omega0{1, i} / (g/1e6); % 转换前单位m/s^2，转换后单位μg
        else
            R(1, i) = R(1, i) * rad2Deg * (3600^2); % 转换前单位rad/s^2，转换后单位°/h^2
            K(1, i) = K(1, i) * rad2Deg * (3600^(3/2)); % 转换前单位rad/s^(3/2)，转换后单位°/h^(3/2)
            B(1, i) = B(1, i) * rad2Deg * 3600; % 转换前单位rad/s，转换后单位°/h
            N(1, i) = N(1, i) * rad2Deg * 60; % 转换前单位rad/sqrt(s)，转换后单位°/sqrt(h)
            Q(1, i) = Q(1, i) * rad2Deg * 3600; % 转换前单位rad，转换后单位″
            Omega0{1, i} = Omega0{1, i} * rad2Deg * 3600; % 转换前单位rad/s，转换后单位°/h
        end
    end
end

%% 绘制曲线
if p.Results.doPlot
    colorSpec = [[0 0 1]; [0 1 0]; [1 0 0]; [0 1 1]; [1 0 1]; [1 1 0]; [0 0 0]]; % blue green red cyan magenta yellow black
    hFig = [0 0];
    accCount = 0;
    gyroCount = 0;
    for i=1:nSens
        if p.Results.isAcc(i)
            accCount = accCount + 1;
            if accCount == 1
                hFig(1, 1) = figure('Name', 'accel Allan std dev');
            end
            figure(hFig(1, 1));
            loglog(tau, sqrt(sigmaSq(:, i)), 'Color', colorSpec(mod(accCount-1, 7)+1, :), 'DisplayName', ['accel ' int2str(accCount) ' calculated value']);
            if accCount == 1
                title('accelerometer Allan standard deviation');
                xlabel('tau(s)');
                ylabel('sigma(tau)(m/s^2)');
                hold on;
            end
            loglog(tau, sqrt(fittedSigmaSq(:, i)), '--', 'Color', colorSpec(mod(accCount-1, 7)+1, :), 'DisplayName', ['accel ' int2str(accCount) ' fitted value']);
        else
            gyroCount = gyroCount + 1;
            if gyroCount == 1
                hFig(1, 2) = figure('Name', 'gyro Allan std dev');
            end
            figure(hFig(1, 2));
            loglog(tau, sqrt(sigmaSq(:, i)), 'Color', colorSpec(mod(gyroCount-1, 7)+1, :), 'DisplayName', ['gyro ' int2str(gyroCount) ' calculated value']);
            if gyroCount == 1
                title('gyro Allan standard deviation');
                xlabel('tau(s)');
                ylabel('sigma(tau)(rad/s)');
                hold on;
            end
            loglog(tau, sqrt(fittedSigmaSq(:, i)), '--', 'Color', colorSpec(mod(gyroCount-1, 7)+1, :), 'DisplayName', ['gyro ' int2str(gyroCount) ' fitted value']);
        end
    end
    for i=1:2
        if hFig(1, i) ~= 0
            figure(hFig(1, i));
            yLimits = ylim;
            color = [128 128 255]/255;
            hReadingLine = zeros(4, 1);
            
            % 绘制读数辅助线
            hReadingLine(1, 1) = line([sqrt(2) sqrt(2)], yLimits, 'LineStyle', '-.', 'Color', ones(1, 3)-color);
            text([sqrt(2) sqrt(2)], yLimits, 'R', 'Color', ones(1, 3)-color);
            hReadingLine(2, 1) = line([3 3], yLimits, 'LineStyle', '-.', 'Color', color);
            text([3 3], yLimits, 'K', 'Color', color);
            hReadingLine(3, 1) = line([1 1], yLimits, 'LineStyle', '-.', 'Color', color);
            text([1 1], yLimits, 'N', 'Color', color);
            hReadingLine(4, 1) = line([sqrt(3) sqrt(3)], yLimits, 'LineStyle', '-.', 'Color', ones(1, 3)-color);
            text([sqrt(3) sqrt(3)], yLimits, 'Q', 'Color', ones(1, 3)-color);
            for j=1:4
                set(get(get(hReadingLine(j, 1), 'Annotation'), 'LegendInformation'), 'IconDisplayStyle', 'off'); % Exclude line from legend
            end
            
            % 绘制拟合阈值线
            if i == 1 % 加速度计
                typedSensNum = accCount;
                typedTaoFitThreshold = tauFitThreshold(1, p.Results.isAcc);
            else % 陀螺
                typedSensNum = gyroCount;
                typedTaoFitThreshold = tauFitThreshold(1, ~p.Results.isAcc);
            end
            for j=1:typedSensNum
                hThresholdLine = line([typedTaoFitThreshold(1, j) typedTaoFitThreshold(1, j)], yLimits, 'LineStyle', ':', 'Color', colorSpec(mod(j-1, 7)+1, :));
                text([typedTaoFitThreshold(1, j) typedTaoFitThreshold(1, j)], yLimits, ['fit threshold ' int2str(j)], 'Color', colorSpec(mod(j-1, 7)+1, :));
                set(get(get(hThresholdLine, 'Annotation'), 'LegendInformation'), 'IconDisplayStyle', 'off'); % Exclude line from legend
            end
            
            legend('show', 'Location', 'Best');
            grid on;
            hold off;
        end
    end
end
toc;
end